#ifndef __MAP_BASE_H__
#define __MAP_BASE_H__

#include <iostream>
using namespace std;

#include <geometry_msgs/Pose.h>
#include <nav_msgs/OccupancyGrid.h>


class MapBase{
    public:
        MapBase(){};
        ~MapBase() = default;

        // 设置地图数据
        void setMap(nav_msgs::OccupancyGrid::Ptr map_ptr){
            grid_ptr_ = map_ptr;
            cout << "Receive a map. width=" << grid_ptr_->info.width 
                << ", height=" << grid_ptr_->info.height
                << ", resolution=" << grid_ptr_->info.resolution
                << endl;
        }

        // 地图坐标转世界坐标
        void mapToWorld(unsigned int mx, unsigned int my, double& wx, double& wy){
            wx = this->getOriginX() + mx * this->getResolution();
            wy = this->getOriginY() + my * this->getResolution();
        }

        // 世界坐标转地图坐标
        void worldToMap(double wx, double wy, unsigned int& mx, unsigned int& my){
            mx = (int)((wx - this->getOriginX()) / this->getResolution());
            my = (int)((wy - this->getOriginY()) / this->getResolution());
            // cout << "wx: " << wx << ", wy: " << wy << ", mx: " << mx << ", my: " << my 
            //     << ", ox: " << this->getOriginX()
            //     << ", oy: " << this->getOriginY() 
            //     << ", res: " << this->getResolution() 
            //     << endl;
        }

        // 地图坐标值是否越界
        bool idxIsVaild(unsigned int mx, unsigned int my){
            if(mx < 0 || mx >= this->getWidth()){
                return false;
            }
            if(my < 0 || my >= this->getHeight()){
                return false;
            }
            return true;    
        }

        bool idxIsVaild(unsigned int idx){
            if(idx < 0 || idx >= this->getSize()){
                return false;
            }
            return true;           
        }

        // 地图坐标转一维坐标
        unsigned int mapToIndex(unsigned int mx, unsigned int my){
            return my * this->getWidth() + mx;
        }

        void indexToMap(unsigned int idx, unsigned int& mx, unsigned int& my){
            my = idx / this->getWidth();
            mx = idx - (my * this->getWidth());
        }

        bool valueIsValid(unsigned int mx, unsigned int my){
            if(!grid_ptr_) return false;
            unsigned int index = mapToIndex(mx, my);
            // cout << "mx: " << mx << ", my: " << my << ", index: " << index  << " | " << getSize() << endl;
            if(index < 0 || index > getSize()){
                return false;
            }
            // cout << "index: " << index << ", value: " << int(grid_ptr_->data[index]) << endl;
            return grid_ptr_->data[index] < 30;
        }

        unsigned char getCost(unsigned int mx, unsigned int my){
            if(!grid_ptr_) return -1;
            unsigned int index = mapToIndex(mx, my);
            if(index < 0 || index > getSize()){
                return -1;
            }
            return grid_ptr_->data[index];
        }

        inline int getSize(){
            return grid_ptr_->data.size();
        }
        inline unsigned int getWidth(){
            return grid_ptr_->info.width;
        }
        inline unsigned int getHeight(){
            return grid_ptr_->info.height;
        }
        inline double getResolution(){
            return grid_ptr_->info.resolution;
        }
        inline double getOriginX(){
            return grid_ptr_->info.origin.position.x;
        }
        inline double getOriginY(){
            return grid_ptr_->info.origin.position.y;
        } 
        inline nav_msgs::OccupancyGrid getOccupancyGrid(){
            return *grid_ptr_;
        }
        inline nav_msgs::OccupancyGrid::Ptr getOccupancyGridPtr(){
            return grid_ptr_;
        }        

    protected:
        nav_msgs::OccupancyGrid::Ptr grid_ptr_ = nullptr;

};

#endif